import sys
sys.path.insert(0, "/home/ninetripod/catkin_ws/src/uav/scripts")

from tests.sim_rplidar import Rplidar_Subscriber, map2img, range_filter, RESOLUTION
import rospy
import json
import time
import cv2
import numpy as np

rospy.init_node("rplidarSubscriber")

radar = Rplidar_Subscriber()
data = []

f = open("/home/ninetripod/catkin_ws/radar_grab.json", 'w+')

t0 = time.time()

while True:
    time.sleep(0.5)
    x = radar.get_data()
    if x is None : continue

    x = x[0:885]
    range_filter(x, 1, 5, 0)
    # print(x)

    data.append(x.tolist())
    # img = map2img(x)
    # cv2.imshow("scan", img)
    # cv2.waitKey(1)
    # print(np.argmin(x) * RESOLUTION / np.pi * 180)
    # print(json.dumps(data))
    
    if time.time() - t0 > 60 * 1:
        break

json.dump(data, f)

f.close()

print("radar grab done")
# cv2.destroyAllWindows()